/*
 * stars_recognition.cpp
 *
 *  Created on: 17.09.2010
 *      Author: pasha
 */

#include "srec.h"
#include <math.h>
using namespace cv;
using namespace std;
#define MAX_INT  ~(1<<31)

namespace sr
{

double sec_to_rad(double sec)
{
	return PI * sec / SECS_IN_DEGREE / 180;
}

int rad_to_secs(double rad)
{
	return round(rad * 180 * SECS_IN_DEGREE / PI);
}

int sidsecs_to_secs(int secs)
{
	return secs * 360 / 24;
}

int DMS_to_secs(int degree, int min, int sec)
{
	return degree * SECS_IN_DEGREE + min * SECS_IN_MINUTE + sec;
}

int HMS_to_secs(int hour, int min, int sec)
{
	return sidsecs_to_secs(DMS_to_secs(hour, min, sec));
}

Position Position::RA_rotation(Position const & old_position, int new_ra_pos) const
{
	Position tmp = old_position;
	int new_ra_secs = old_position.ra.to_secs() - new_ra_pos;
	if (new_ra_secs < 0)
	{
		new_ra_secs += 360 * SECS_IN_DEGREE;
	}
	tmp.ra = Position::RA(new_ra_secs);
	return tmp;
}

Position Position::DEC_rotation(Position const & old_position,
		int new_dec_position) const
{
	int old_ra = old_position.ra.to_secs();
	int old_dec = 90 * SECS_IN_DEGREE - old_position.dec.to_secs();
	double A = old_ra * PI / 180 / SECS_IN_DEGREE;
	double z = old_dec * PI / 180 / SECS_IN_DEGREE;
	double phi = (90 * SECS_IN_DEGREE - new_dec_position) * PI / 180
			/ SECS_IN_DEGREE;
	double delta = asin(cos(z) * sin(phi) - sin(z) * cos(phi) * cos(A));
	double t = asin(sin(z) * sin(A) / cos(delta));
	double lp = cos(delta) * cos(t);
	double rp = cos(z) * cos(phi) + sin(z) * sin(phi) * cos(A);
	if (lp * rp < 0 && 2 * PI - t >= 0)
	{
		t = 2 * PI - t;
	}
	return Position(rad_to_secs(t), rad_to_secs(delta));
}

Position Position::get_rotation(Position const & new_center) const
{
	return DEC_rotation(RA_rotation(*this, new_center.ra.to_secs()),
			new_center.dec.to_secs());
}

Position::RA::RA(int secs)
{
	this->deg = secs / SECS_IN_DEGREE;
	secs = secs % SECS_IN_DEGREE;
	this->min = secs / SECS_IN_MINUTE;
	this->sec = secs % 60;
}

int Position::RA::to_secs() const
{
	return DMS_to_secs(deg, min, sec);
}

Position::DEC::DEC(int secs)
{
	if (secs < 0)
	{
		sign = -1;
		secs *= -1;
	}
	else
	{
		sign = 1;
	}
	deg = secs / SECS_IN_DEGREE;
	secs = secs % SECS_IN_DEGREE;
	min = secs / SECS_IN_MINUTE;
	sec = secs % 60;
}

int Position::DEC::to_secs() const
{
	return sign * (DMS_to_secs(deg, min, sec));
}

int Position::get_ra_shift(Position const & other_pos) const
{
	return (other_pos.get_rotation(*this).ra.to_secs());
}

int Position::get_dec_shift(Position const & other_pos) const
{
	return (other_pos.get_rotation(*this).dec.to_secs());
}

int Position::distance(Position const & other_pos) const
{
	return round(sqrt((long long) get_dec_shift(other_pos) * get_dec_shift(
			other_pos) + (long long) get_ra_shift(other_pos) * get_ra_shift(
			other_pos)));
}

long long Position::sqr_of_distance(Position const & other_pos) const
{
	return (long long) get_dec_shift(other_pos) * get_dec_shift(other_pos)
			+ (long long) get_ra_shift(other_pos) * get_ra_shift(other_pos);
}

Position::Position()
{

}

Position::Position(int ra_secs, int dec_sec, bool equatorial) :
	ra(), dec()
{
	if (!equatorial)
	{
		if (ra_secs < 0)
		{
			ra_secs += 360 * SECS_IN_DEGREE;
		}
	}
	if (dec_sec >= 0)
	{
		dec.sign = 1;
	}
	else
	{
		dec.sign = -1;
		dec_sec *= -1;
	}
	dec.deg = dec_sec / SECS_IN_DEGREE;
	dec_sec %= SECS_IN_DEGREE;
	dec .min = dec_sec / SECS_IN_MINUTE;
	dec.sec = dec_sec % SECS_IN_MINUTE;
	ra.deg = ra_secs / SECS_IN_DEGREE;
	ra_secs %= SECS_IN_DEGREE;
	ra .min = ra_secs / 60;
	ra.sec = ra_secs % 60;
}

Position::Position(int ra_hour, int ra_min, int ra_sec, int dec_deg,
		int dec_min, int dec_sec, int dec_sign) :
	ra(HMS_to_secs(ra_hour, ra_min, ra_sec)), dec(DMS_to_secs(dec_deg, dec_min,
			dec_sec) * dec_sign)
{
}

Star::Star() :
	Position(), color(CV_RGB(255, 255, 255))
{
}

Star::Star(int ra_deg, int ra_min, int ra_sec, int dec_deg, int dec_min,
		int deg_sec, double magn, int HD, cv::Scalar color, int sign) :
	Position(ra_deg, ra_min, ra_sec, dec_deg, dec_min, deg_sec, sign), HD(HD),
			magn(magn), color(color)
{
}

Star::Star(char * entry, std::vector<int> const & marked)
{
	ra = Position::RA(HMS_to_secs(10 * (entry[75] - '0') + entry[76] - '0', 10
			* (entry[77] - '0') + entry[78] - '0', 10 * (entry[79] - '0')
			+ entry[80] - '0'));
	int dec_sign = 1;
	if (entry[83] == '-')
	{
		dec_sign = -1;
	}
	dec = Position::DEC(dec_sign * DMS_to_secs(10 * (entry[84] - '0')
			+ entry[85] - '0', 10 * (entry[86] - '0') + entry[87] - '0', 10
			* (entry[88] - '0') + entry[89] - '0'));
	magn = (entry[103] - '0') + 0.1 * (entry[105] - '0') + 0.01 * (entry[107]
			- '0');
	is_marked = false;
	HD = 0;
	for (int k = 25; k <= 30; k++)
	{
		if (entry[k] != 32)
		{
			HD = HD * 10 + entry[k] - '0';
		}
	}

	color = CV_RGB(255,255,255);
	if (std::find(marked.begin(), marked.end(), HD) != marked.end())
	{
		color = CV_RGB(255, 0, 0);
	}
}

int Star::get_rad(int base, double magn) const
{
	return round(base * pow(2, magn - this->magn));
}

Position const & Position::get_zero()
{
	static Position tmp = Position(0, 0);
	return tmp;
}

Sphere::Sphere(vector<Star> & stars) :
	sphere(360, vector<list<Star*> > (181, list<Star*> ()))
{
	for (vector<Star>::iterator i = stars.begin(); i != stars.end(); ++i)
	{
		sphere[(*i).ra.deg][(*i).dec.deg * (*i).dec.sign + 90] .push_back(&(*i));
	}
}

list<Star*> const & Sphere::get_list(Position const & pos) const
{
	return sphere[pos.ra.deg][pos.dec.deg * pos.dec.sign + 90];
}

Position Position::move_position(int ra, int dec) const
{
	if (ra < 0)
	{
		ra += SECS_IN_DEGREE * 360;
	}
	return Position(ra, dec).get_rotation(Position::get_zero().get_rotation(
			*this));
}
typedef list<Star*>::const_iterator degree_iterator;

Star * Sphere::get_nearest_in_deg(Position const & pos,
		list<Star*> const & degree) const
{
	if (degree.size() == 0)
		return 0;

	int min_distance = MAX_INT;
	Star * nearest;
	for (degree_iterator i = degree.begin(); i != degree.end(); i++)
	{
		if (pos.distance(**i) < min_distance)
		{
			min_distance = pos.distance(**i);
			nearest = *i;
		}
	}
	return nearest;
}

Star * Sphere::get_nearest(Position const & position) const
{
	int min_distance = MAX_INT;
	Star * nearest;
	Star * cur_star;
	for (int i = -1; i <= 1; i++)
	{
		for (int k = -1; k <= 1; k++)
		{
			cur_star = get_nearest_in_deg(position, get_list(position));
			if (cur_star && cur_star->distance(position) < min_distance)
			{
				nearest = cur_star;
			}
		}
	}
	return nearest;
}

class CoordsConvertor
{
	double angle;
	Point2f new_center;
	double mult;
public:
	CoordsConvertor(double angle, Point2f new_center, double mult) :
		angle(angle), new_center(new_center), mult(mult)
	{
	}
	Point2i operator()(Point2f old)
	{
		Point2i tmp;
		tmp.x = cvRound(((double) old.x * cos(angle) - (double) old.y * sin(
				angle) + (double) new_center.x) * mult);
		tmp.y = cvRound(((double) old.x * sin(angle) + (double) old.y * cos(
				angle) + (double) new_center.y) * mult);
		return tmp;
	}
};

long long get_mistake(Point2f first, Point2f second, Star* first_s,
		Star * second_s, vector<Point2f> & stars, Sphere const & sky)
{
	Point2i second_s_rel;
	second_s_rel.x = first_s->get_ra_shift(*second_s);
	second_s_rel.y = first_s->get_dec_shift(*second_s);
	double angle_on_sky = atan((double) second_s_rel.y / second_s_rel.x);
	if (second_s_rel.x < 0)
	{
		angle_on_sky += PI;
	}
	Point2f second_rel;
	second_rel.x = second.x - first.x;
	second_rel.y = second.y - first.y;

	double angle_on_image = atan(second_rel.y / second_rel.x);
	if (second_rel.x < 0)
	{
		angle_on_image += PI;
	}

	double angle = angle_on_image - angle_on_sky;

	double mult = first_s->distance(*second_s) / sqrt((second.x - first.x)
			* (second.x - first.x) + (second.y - first.y)
			* (second.y - first.y));

	CoordsConvertor to_new(angle, first, mult);

	long long sum = 0;
	for (vector<Point2f>::const_iterator i = stars.begin(); i != stars.end(); ++i)
	{
		Point2i cur_pos = to_new(*i);
		Star * nearest = sky.get_nearest(first_s->move_position(cur_pos.x,
				cur_pos.y));
		if (nearest == 0)
			return MAX_INT;
		sum += nearest->distance(first_s->move_position(cur_pos.x, cur_pos.y));
	}
	return sum;
}

vector<Star> find_match(vector<cv::Point2f> const & stars,
		Sphere const & sky)
{
	Point2f first = stars[0];
	Point2f second = stars[1];
	return vector<Star> ();
}

}

